
#include <agv_peripheral_controller/roller/motor_control.h>
namespace peripheral_roller
{
	CMotorControl::CMotorControl()
	{
		m_init = false;
		m_radius = 0.1f;
		m_state = MonitorState::Normal;
	}

	CMotorControl::~CMotorControl()
	{
		m_quit_mutex.unlock();
		m_thread.join();
	}

	bool CMotorControl::init()
	{
		m_periphMotorInfo.motor_name = ROLLER_JOINT;
		m_jointStateSub = m_privNh.subscribe("/base/joint_states", 1, &CMotorControl::jointStateCallback, this);
		m_driverInfoSub = m_privNh.subscribe("/base/driversInfo", 1, &CMotorControl::driversInfoCallback, this);
		m_clientHalt = m_privNh.serviceClient<std_srvs::srv::Trigger>("/base/driver/halt");
		m_clientRecover = m_privNh.serviceClient<std_srvs::srv::Trigger>("/base/driver/recover");
		m_rollerJointPub = m_privNh.advertise<std_msgs::msg::Float64>("/base/controllers/roller_controller/command", 10);
		m_privNh.param<float>("/agv_peripheral_controller/roller/rollerRadius", m_radius, 0.1f);
		m_radius = abs(m_radius);
		m_radius = m_radius < 0.0001f ? 0.1 : m_radius;
		DEBUG_OUT("m_radius=" << m_radius);

		m_quit_mutex.lock();
		m_thread = std::thread(thread_fun, this);
		m_init = true;
		return true;
	}

	// 设置旋转速度 vel> 0:正转  vel<0:反转
	bool CMotorControl::rotate(float vel)
	{
		// 错误1:半径为0
		if (std::abs(m_radius) < 0.0001f)
		{
			m_fVel = 0.0f;
			DEBUG_ERROR_OUT("error:CMotorControl:m_radius is 0.0f");
			return false;
		}

		// 错误2：速度过大
		if (std::abs(vel) > 1)
		{
			m_fVel = 0.0f;
			DEBUG_ERROR_OUT("error:CMotorControl vel is too big:vel=" << vel);
			return false;
		}

		// 错误3:速度非法
		if (std::isnan(vel))
		{
			m_fVel = 0.0f;
			DEBUG_ERROR_OUT("error:CMotorControl:rotate vel is nan");
			return false;
		}

		bool err = fault();
		vel = !err ? vel : 0.0f;

		m_fVel = vel;
		if (err)
		{
			DEBUG_ERROR_OUT("error:CMotorControl is not normal,can not rotate");
		}

		return !err;
	}

	// 电机停止
	bool CMotorControl::stop()
	{
		rotate(0.0f);
		return true;
	}
	// 电机是否报错
	bool CMotorControl::fault()
	{
		bool normal = m_state == MonitorState::Normal && motorReady() && motorEnable() && !motorError();
		return !normal;
	}

	std::string CMotorControl::faultMsg()
	{
		bool e1 = m_state == MonitorState::JointTimeOut;
		bool e2 = m_state == MonitorState::DriverTimeOut;
		bool e3 = !motorReady();
		bool e4 = !motorEnable();
		bool e5 = motorError();

		std::string msg = "roller motor:";
		msg += e1 ? "joint time out; " : "";
		msg += e2 ? "driver info time out; " : "";
		msg += e3 ? "not ready; " : "";
		msg += e4 ? "not enable; " : "";
		msg += e5 ? "motor driver error; " : "";
		return msg;
	}

	//
	// 试图清除电机错误
	//
	bool CMotorControl::clearFault()
	{
		if (!m_init)
		{
			DEBUG_ERROR_OUT("tryRecoverMotor: it is not init");
			return false;
		}
		std_srvs::srv::Trigger trig;
		bool ret = m_clientRecover.call(trig);
		ROS_INFO_STREAM("MotorControl call recover service result:" << (ret ? "ok" : "failed"));
		return ret;
	}
	//
	//  试图下使能
	//
	bool CMotorControl::tryHaltMotor()
	{
		if (!m_init)
		{
			DEBUG_ERROR_OUT("tryHaltMotor: it is not init");
			return false;
		}
		std_srvs::srv::Trigger trig;
		bool ret = m_clientHalt.call(trig);
		ROS_INFO_STREAM("MotorControl call halt service result:" << (ret ? "ok" : "failed"));
		return ret;
	}

	/** @brief Callback function for the Joint State subscriber */
	void CMotorControl::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &msg)
	{
		auto jointState = *msg;
		for (std::size_t i = 0; i < jointState.name.size(); i++)
		{
			if (jointState.name[i] == ROLLER_JOINT)
			{
				m_jointState.Update(jointState);
				ROS_INFO_STREAM_THROTTLE(1, "jointStateCallback:" << jointState);
			}
		}
	}

	void CMotorControl::checkJointState()
	{
		MonitorState state = MonitorState::Normal;
		if (m_jointState.TimeOut(3000)) // 1s
		{
			state = MonitorState::JointTimeOut;
			DEBUG_ERROR_OUT("[CMotorControl] check joint state timeout");
		}
		else if (m_driverInfo.TimeOut(3000)) // 1s
		{
			state = MonitorState::DriverTimeOut;
			DEBUG_ERROR_OUT("[CMotorControl] check driver info timeout");
		}
		m_state = state;
	}

	void CMotorControl::driversInfoCallback(const agv_msgs::msg::DriversInfo::ConstSharedPtr &data)
	{
		auto driversInfo = *data;

		uint16_t statusWord = 0x0;
		static uint16_t lastStatusWord = statusWord;

		for (std::size_t i = 0; i < driversInfo.drivers.size(); i++)
		{
			statusWord = driversInfo.drivers[i].statusWord;

			if (driversInfo.drivers[i].jointName == ROLLER_JOINT)
			{
				// 更新一下驱动消息的时间戳
				m_driverInfo.Update(driversInfo);
				// motorStatus.movingState = statusWord & 0x400;
				m_periphMotorInfo.enableStatus = (statusWord & 0x07) == 0x07;
				m_periphMotorInfo.homeFinished = statusWord & 0x8000;
				m_periphMotorInfo.errStatus = statusWord & 0x8;
				m_periphMotorInfo.ready = driversInfo.drivers[i].ready;

				if (statusWord != lastStatusWord)
				{
					ROS_INFO_STREAM("roller motor statusWord=" << (int)(statusWord)
															   << ",enable=" << (int)m_periphMotorInfo.enableStatus
															   << ",homeState=" << (int)m_periphMotorInfo.homeFinished);
					lastStatusWord = statusWord;
				}
				break;
			}
		}
	}

	//
	// 电机是否就绪状态
	//
	bool CMotorControl::motorReady()
	{
		if (!m_init)
		{
			DEBUG_ERROR_OUT("motorReady: it is not init");
			return false;
		}

		bool ready = m_periphMotorInfo.ready;
		ROS_INFO_STREAM_THROTTLE(0.5, "roller motor ready=" << ready);
		return ready;
	}

	//
	// 电机是否使能状态
	//
	bool CMotorControl::motorEnable()
	{
		if (!m_init)
		{
			DEBUG_ERROR_OUT("motorEnable: it is not init");
			return false;
		}
		bool enable = m_periphMotorInfo.enableStatus;
		ROS_INFO_STREAM_THROTTLE(0.5, "roller motor enable=" << enable);
		return enable;
	}
	//
	// 电机是否报错
	//
	bool CMotorControl::motorError()
	{
		if (!m_init)
		{
			DEBUG_ERROR_OUT("motorError: it is not init");
			return false;
		}
		bool motorErr = m_periphMotorInfo.errStatus;
		ROS_INFO_STREAM_THROTTLE(0.5, "roller motor error=" << motorErr);
		return motorErr;
	}

	std::vector<agv_msgs::msg::PeriphMotorInfo> CMotorControl::motorInfoCollect()
	{
		// periphStatus.periphStateInfo = 0;
		std::vector<agv_msgs::msg::PeriphMotorInfo> info;
		info.push_back(m_periphMotorInfo);
		return info;
	}

	void CMotorControl::setVel(float radPerSecond)
	{
		if (!m_init)
		{
			DEBUG_ERROR_OUT("setVel: it is not init");
			return;
		}

		// 错误:速度非法
		if (std::isnan(radPerSecond))
		{
			radPerSecond = 0.0f;
			DEBUG_ERROR_OUT("error:CMotorControl:radPerSecond  is nan");
		}

		// DEBUG_OUT("CMotorControl(rad/s): vel=" << vel);
		std_msgs::msg::Float64 target;
		target.data = radPerSecond;
		m_rollerJointPub.publish(target);
	}

	bool CMotorControl::rotateState()
	{
		return abs(m_fVel) > 0.0001f;
	}

	void CMotorControl::thread_cycle()
	{
		checkJointState();

		m_radius = m_radius < 0.0001f ? 0.1 : m_radius;
		setVel(m_fVel / m_radius);
	}

	void CMotorControl::thread_fun(CMotorControl *pThis)
	{
		if (!pThis)
			return;

		while (!pThis->m_quit_mutex.try_lock_for(std::chrono::milliseconds(20)))
		{
			pThis->thread_cycle();
		}
	}
}; // end namespace
